{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import torch\n",
    "from torch.autograd import Variable\n",
    "from mpc import mpc\n",
    "from mpc.mpc import QuadCost, LinDx"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Initial mean(cost): 3.9041e+01\n",
      "| iter | mean(cost) | ||full_du||_max | mean(alphas) | total_qp_iters |\n",
      "| 0 | 6.6806e+00 | 1.94e+00 | 1.00e+00 | 16 |\n",
      "| 1 | 6.4417e+00 | 2.39e+00 | 6.00e-01 | 15 |\n",
      "| 2 | 4.5778e+00 | 6.19e-01 | 1.00e+00 | 13 |\n",
      "| 3 | 4.4537e+00 | 2.90e-01 | 1.00e+00 | 13 |\n",
      "| 4 | 4.4527e+00 | 6.44e-02 | 1.00e+00 | 11 |\n",
      "| 5 | 4.4527e+00 | 1.92e-07 | 1.00e+00 | 6 |\n",
      "| 6 | 4.4527e+00 | 1.96e-07 | 1.00e+00 | 8 |\n",
      "| 7 | 4.4527e+00 | 5.30e-07 | 5.20e-01 | 10 |\n",
      "| 8 | 4.4527e+00 | 7.19e-08 | 6.00e-01 | 8 |\n"
     ]
    }
   ],
   "source": [
    "torch.manual_seed(0)\n",
    "\n",
    "n_batch, n_state, n_ctrl, T = 2, 3, 4, 5\n",
    "n_sc = n_state + n_ctrl\n",
    "\n",
    "# Randomly initialize a PSD quadratic cost and linear dynamics.\n",
    "C = torch.randn(T*n_batch, n_sc, n_sc)\n",
    "C = torch.bmm(C, C.transpose(1, 2)).view(T, n_batch, n_sc, n_sc)\n",
    "c = torch.randn(T, n_batch, n_sc)\n",
    "\n",
    "alpha = 0.2\n",
    "R = (torch.eye(n_state)+alpha*torch.randn(n_state, n_state)).repeat(T, n_batch, 1, 1)\n",
    "S = torch.randn(T, n_batch, n_state, n_ctrl)\n",
    "F = torch.cat((R, S), dim=3)\n",
    "\n",
    "# The initial state.\n",
    "x_init = torch.randn(n_batch, n_state)\n",
    "\n",
    "# The upper and lower control bounds.\n",
    "u_lower = -torch.rand(T, n_batch, n_ctrl)\n",
    "u_upper = torch.rand(T, n_batch, n_ctrl)\n",
    "\n",
    "x_lqr, u_lqr, objs_lqr = mpc.MPC(\n",
    "    n_state=n_state,\n",
    "    n_ctrl=n_ctrl,\n",
    "    T=T,\n",
    "    u_lower=u_lower, \n",
    "    u_upper=u_upper,\n",
    "    lqr_iter=20,\n",
    "    verbose=1,\n",
    "    backprop=False,\n",
    "    exit_unconverged=False,\n",
    ")(x_init, QuadCost(C, c), LinDx(F))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
